package com.gitee.wsl.mathematics.complex.quaternion.ext

import com.gitee.wsl.mathematics.vector.ext.dot
import com.gitee.wsl.mathematics.complex.quaternion.Quaternion
import com.gitee.wsl.mathematics.vector.vec3.Vector3

/** Compute a cross product between a quaternion and a vector.
 *  @see gtx_quaternion */
fun<T:Number> Quaternion<T, *>.cross(v: Vector3<T,*>):  Vector3<T,*> {
    // inverse(q)
    val dot = this dot this
    val w = w / dot
    val x = -x / dot
    val y = -y / dot
    val z = -z / dot
    // inverse(q) * v
    val uvX = y * v.z - v.y * z
    val uvY = z * v.x - v.z * x
    val uvZ = x * v.y - v.x * y
    val uuvX = y * uvZ - uvY * z
    val uuvY = z * uvX - uvZ * x
    val uuvZ = x * uvY - uvX * y
    val resX = v.x + (uvX * w + uuvX) * 2f
    val resY = v.y + (uvY * w + uuvY) * 2f
    val resZ = v.z + (uvZ * w + uuvZ) * 2f
    return createVec3(resX, resY, resZ)
}

/** Compute a cross product between a vector and a quaternion.
 *  @see gtx_quaternion */
fun<T:Number,V:Vector3<T,V>> V.cross(q: Quaternion<T, *>):V{
    // q * v
    val uvX = q.y * z - y * q.z
    val uvY = q.z * x - z * q.x
    val uvZ = q.x * y - x * q.y
    val uuvX = q.y * uvZ - uvY * q.z
    val uuvY = q.z * uvX - uvZ * q.x
    val uuvZ = q.x * uvY - uvX * q.y
    val resX = x + (uvX * q.w + uuvX) * 2f
    val resY = y + (uvY * q.w + uuvY) * 2f
    val resZ = z + (uvZ * q.w + uuvZ) * 2f
    return create(resX, resY, resZ)
}